#include <WiFi.h>
#include <ArtronShop_LineNotify.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <MMA8453_n0m1.h>

const char* ssid = "Nuxxtp.";                                     //ใส่ ชื่อ Wifi ที่จะเชื่อมต่อ
const char* password = "Nuxxtp12";                                //ใส่ รหัส Wifi
#define LINE_TOKEN "t0T0rl6R6769QxQUrqL1jkTXyL20hBKBvywCfhhtehF"  //ใส่ รหัส TOKEN ที่ได้มาจากข้างบน

#define BUZZER_PIN 19
boolean flag = true;

MMA8453_n0m1 accel;

LiquidCrystal_I2C lcd(0x27, 20, 4);  //จอ
unsigned long prevTime_T1 = millis();



int button1 = 0;
int button2 = 0;
int button3 = 0;
int button4 = 0;
int button5 = 0;
int help = 0;
int help_count = 0;
int leftButtonState = 0;
int rightButtonState = 0;
int sudnut = 0;
int pwmSpeed = 0;
long delayTime = 0;

int count = 0;
int sensor = 34;
int ENA = 2;
int IN1 = 26;
int IN2 = 27;
int InterStart = 0;
int ENB = 4;
int IN11 = 14;
int IN22 = 12;

const int freq = 500;
const int pwmChannel = 0;
const int pwmChanne2 = 1;
const int resolution = 8;
int dutyCycle = 200;



void setup() {
  Serial.begin(115200);  // เริ่มต้นใช้ Serial ที่ความเร็ว 115200
  while (!Serial) { delay(100); }

  // We start by connecting to a WiFi network

  Serial.println();
  Serial.println("******************************************************");
  Serial.print("Connecting to ");
  Serial.println(ssid);

  WiFi.begin(ssid, password);  // เริ่มต้นเชื่อมต่อ WiFi

  while (WiFi.status() != WL_CONNECTED) {  // วนลูปหากยังเชื่อมต่อ WiFi ไม่สำเร็จ
    delay(500);
    Serial.print(".");
  }

  Serial.println("");
  Serial.println("WiFi connected");
  Serial.println("IP address: ");
  Serial.println(WiFi.localIP());

  LINE.begin(LINE_TOKEN);  // เริ่มต้นใช้ LINE Notify

  accel.setI2CAddr(0x1C);   //change your device address if necessary, default is 0x1C
  accel.dataMode(true, 2);  //enable highRes 10bit, 2g range [2g,4g,8g]
  Serial.println("XYZ Data Example");
  pinMode(32, INPUT_PULLUP);  //button1
  pinMode(13, INPUT_PULLUP);  //button2
  pinMode(5, INPUT_PULLUP);   //button3
  pinMode(15, INPUT_PULLUP);  //button4
  pinMode(16, INPUT_PULLUP);  //button5

  pinMode(19, OUTPUT);  //buzzer
  pinMode(34, INPUT);   //sensor

  //motor 1
  pinMode(ENA, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  //motor 2
  pinMode(ENB, OUTPUT);
  pinMode(IN11, OUTPUT);
  pinMode(IN22, OUTPUT);

  ledcSetup(pwmChannel, freq, resolution);
  ledcAttachPin(ENA, pwmChannel);
  ledcAttachPin(ENB, pwmChanne2);



  // lcd.begin();
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("x:");
  lcd.setCursor(7, 0);
  lcd.print("y:");
  lcd.setCursor(14, 0);
  lcd.print("z:");
  lcd.setCursor(0, 1);
  lcd.print(" SPEED WALK:");
  lcd.setCursor(0, 2);
  lcd.print(" DELAY WALK:");
  lcd.setCursor(0, 3);
  lcd.print(" STATUS:");
}

void loop() {
  int sensor_Value = digitalRead(34);
  unsigned long currentMillis = millis();

  // Execute a task
  Serial.println("Task executed");
  btnStartStop();
 
  // btnRight();
  if (currentMillis - prevTime_T1 > delayTime) {
    if (InterStart == 0) {
       btnLeft();
      InterStart = 1;
    } else if (InterStart == 1) {
      stop();
      InterStart = 0;
    }
     prevTime_T1 = currentMillis;
    // Update the timestamp
  }
  if (sensor_Value == LOW) {
    count++;
    Serial.print("Count = ");
    Serial.println(count);
  }
  lcd.setCursor(8, 3);
  lcd.print("C=");
  lcd.print(count);

  displayXYZ();
  btnSpeed();
  btnDelay();
}

void displayXYZ() {

  if (((accel.x() > 200 || accel.x() < -250) || (accel.y() > 250 || accel.y() < -250)) and help_count <= 5) {
    LINE.send("กำลังล้ม❗❗");
    digitalWrite(19, HIGH);
    help_count += 1;
    help = 1;
  } else if (help_count >= 5) {
    help_count = 0;
  } else if (((accel.x() > 80 || accel.x() < -80) || (accel.y() > 15 || accel.y() < -15)) and help == 1) {
    LINE.send("ช่วยเหลือแล้ว✅✅");
    help = 0;
  } else {
    digitalWrite(19, LOW);
  }

  accel.update();
  lcd.setCursor(2, 0);
  lcd.print("    ");
  lcd.setCursor(2, 0);
  lcd.print(accel.x());
  delay(50);
  lcd.setCursor(9, 0);
  lcd.print("    ");
  lcd.setCursor(9, 0);
  lcd.print(accel.y());
  delay(50);
  lcd.setCursor(16, 0);
  lcd.print("     ");
  lcd.setCursor(16, 0);
  lcd.print(accel.z());
  delay(50);
}
void btnSpeed() {
  //button1
  // Serial.print("SS = ");
  // Serial.println(digitalRead(32));
  Serial.print("btn1 = ");
  Serial.println(button1);
  if (digitalRead(32) == 0) {
    button1 += 1;
    lcd.setCursor(12, 1);
    lcd.print("     ");
    delay(150);
  }
  if (button1 == 0) {
    lcd.setCursor(12, 1);
    lcd.print(" 0  M/S");  //0
    pwmSpeed = 0;
    setMotorSpeed();
  } else if (button1 == 1) {
    lcd.setCursor(12, 1);
    lcd.print(" 5  M/S");  //64
    pwmSpeed = 100;
    setMotorSpeed();
  } else if (button1 == 2) {
    lcd.setCursor(12, 1);
    lcd.print(" 10 M/S");  //172
    pwmSpeed = 172;
    setMotorSpeed();
  } else if (button1 == 3) {
    lcd.setCursor(12, 1);
    lcd.print(" 15 M/S");  //191
    pwmSpeed = 191;
    setMotorSpeed();
  } else if (button1 == 4) {
    lcd.setCursor(12, 1);
    lcd.print(" 20 M/S");  //255
    pwmSpeed = 255;
    setMotorSpeed();
  } else {
    button1 = 0;
  }
}

void btnStartStop() {
  //button3
  // Serial.print("SS = ");
  // Serial.println(digitalRead(0));
  if (digitalRead(5) == 0) {
    button3 += 1;
    lcd.setCursor(14, 3);
    lcd.print("     ");
    delay(150);
    if (button3 > 1) {
      button3 = 0;
    }
    if (button3 == 0) {
      lcd.setCursor(12, 3);
      lcd.print(" STOP ");
      lcd.setCursor(8, 3);
      lcd.print("C=");
      lcd.print(count);
      Serial.println("STOP");
      stop();
    } else if (button3 == 1 and sudnut == 0) {
      lcd.setCursor(12, 3);
      lcd.print("START");
      start();
      Serial.println("START");
    }
  } else if (button3 == 0) {
    lcd.setCursor(12, 3);
    lcd.print(" STOP ");
  }
  // lcd.setCursor(12, 3);
  // lcd.print(digitalRead(5));
  Serial.println(digitalRead(5));
}

void btnDelay() {
  //button2
  // Serial.print("SS = ");
  // Serial.println(digitalRead(13));
  // Serial.println(button2);
  if (digitalRead(13) == 0) {
    button2 += 1;
    lcd.setCursor(12, 2);
    lcd.print("     ");
    delay(150);
  }
  if (button2 == 0) {
    lcd.setCursor(12, 2);
    lcd.print(" 1  S");  //delay 100 s
    delayTime = 1000;
    // delayMotor();
  } else if (button2 == 1) {
    lcd.setCursor(12, 2);
    lcd.print(" 2  S");  //delay 200 s
    delayTime = 2000;
    // delayMotor();
  } else if (button2 == 2) {
    lcd.setCursor(12, 2);
    lcd.print(" 3  S");  //delay 300 s
    delayTime = 3000;
    // delayMotor();
  } else if (button2 == 3) {
    lcd.setCursor(12, 2);  //delay 400 s
    lcd.print(" 4  S");
    delayTime = 4000;
    // delayMotor();
  } else if (button2 == 4) {
    lcd.setCursor(12, 2);  //delay 500 s
    lcd.print(" 5  S");
    delayTime = 5000;
    // delayMotor();
  } else {
    button2 = 0;
  }
}

void btnRight() {
  //button4
  // Serial.print("SS = ");
  // Serial.println(digitalRead(15));
  Serial.println(button4);
  if (digitalRead(15) == 0 and button3 == 1)  //right
  {
    sudnut = 1;
    lcd.setCursor(12, 3);
    lcd.print("RIGHT ");
    right();
  }
  else if(digitalRead(15)== 1 and button3 == 1){
    lcd.setCursor(12, 3);
    lcd.print("START ");
    start();
    sudnut = 0;
  }
}

void btnLeft() {
  //button5
  // Serial.print("SS = ");
  // Serial.println(digitalRead(16));
  Serial.println(button5);
  if (digitalRead(16) == 0 and button3 == 1)  //left
  {
    sudnut = 1;
    lcd.setCursor(12, 3);
    lcd.print("LEFT ");
    left();
  } else if (digitalRead(15) == 0 and button3 == 1)  //right
  {
    sudnut = 1;
    lcd.setCursor(12, 3);
    lcd.print("RIGHT ");
    right();
  } else if (digitalRead(15) == 1 and digitalRead(16) == 1 and button3 == 1) {
    lcd.setCursor(12, 3);
    lcd.print("START ");
    start();
    sudnut = 0;
  }
}

//motor
void start() {

  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  analogWrite(ENA, pwmSpeed);
  digitalWrite(IN11, HIGH);
  digitalWrite(IN22, LOW);
  analogWrite(ENB, pwmSpeed);
}

void stop() {
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN11, LOW);
  digitalWrite(IN22, LOW);
}

void setMotorSpeed() {
  analogWrite(ENA, pwmSpeed);
  analogWrite(ENB, pwmSpeed);
}

void delayMotor() {

  
    // Your code for starting the motors
    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);
    analogWrite(ENA, pwmSpeed);

    digitalWrite(IN11, HIGH);
    digitalWrite(IN22, LOW);
    analogWrite(ENB, pwmSpeed);



    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);
    analogWrite(ENA, pwmSpeed);


    digitalWrite(IN11, HIGH);
    digitalWrite(IN22, LOW);
    analogWrite(ENB, pwmSpeed);
  
}
void right() {

  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  analogWrite(ENA, pwmSpeed);
  digitalWrite(IN11, LOW);
  digitalWrite(IN22, HIGH);
  analogWrite(ENB, pwmSpeed);
}
void left() {

  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  analogWrite(ENA, pwmSpeed);
  digitalWrite(IN11, HIGH);
  digitalWrite(IN22, LOW);
  analogWrite(ENB, pwmSpeed);
}
